
clc;
clear;
r = 9999;
h = 0.001;
d = r*h;
d0 = h*d;
x1 = 0;
x2 = 0;
x3 = 0;

for i = 1 : 4000
 T(i) = 0.001*(i-1);
 vt = sin(1*T(i));
 g = x1 - vt + h*x2;
 absg = abs(g);
 a0 = sqrt(d*d+8*r*absg);

 if absg > d0
    a = x2 + (a0-d)/2*sign(g);
 else
    a = x2 + g/h;
 end

 absa = abs(a);

 if absa >d
    y = -r*sign(a);
 else
    y = -r*a/d;
 end
 
 x10(i) = vt;
 
 for j = 1 : 10
 Tj(i*10 + j) = 0.001*(i*10 + j -1)/10;
 
 x13(i*10 + j) = y;
 x12(i*10 + j) = x2 + y*h/10;
 x11(i*10 + j) = x1 + x12(i*10 + j)*h/10;
%  
 x1 = x11(i*10 + j);
 x2 = x12(i*10 + j);
 
 end
 
end


 figure;
 plot(T(:),x10(:),'*-');
 hold on;
 grid;
 plot(Tj(:),x11(:),'o-');
 
 figure;
 plot(Tj(:),x12(:),'*-');
 grid;

 
 figure;
 plot(Tj(:),x13(:),'o');
 grid;

 
 
